/*
io_config.c
config io port,io read and write
io pin number
from 0~23
designed by lunanting
----------------------
2020-03-20 17:13:30
creat file
---------------------

*/
#define USE_IO_EXT_SOFTWARE_V2  1

#include "io_config.h"
#include "string.h"
#include "cli_cmd.h"
#include "aos/hal/gpio.h"
#include "stdlib.h"
#include "aos/kernel.h"
#include "tool.h"

bool is_io_board_working = false;
static void (*io_boardPower_manage)(void)=NULL;
void xp_io_board_tryConnect(void *arg);

typedef struct {
    uint8_t             configDevCrlNum;        //子板控制的设备数(单个设备控制由4个引脚组成（正转、反转、脉冲、零位），单个子板最多支持24/4=6个设备)
    uint8_t             pin_zero[MAX_REG_DEV_NUMS];            //零点传感器引脚
    uint8_t             pin_pulse[MAX_REG_DEV_NUMS];           //脉冲计数引脚
    uint8_t             pin_CW[MAX_REG_DEV_NUMS];              //正向控制引脚
    uint8_t             pin_CCW[MAX_REG_DEV_NUMS];             //反向控制引脚
    uint32_t            configData[MAX_REG_DEV_NUMS];          //配置的数据
} Type_DevCrlPin_Def;

Type_DevCrlPin_Def      crlDev[6] = {0};        //最多支持6块子板


#if USE_IO_EXT_SOFTWARE_V2
/*使用新版io子板程序,非canopen*/
/**************************************************************************************************/
#define WRITE_REG(REG, VAL)   ((REG) = (VAL))
#define READ_REG(REG)         ((REG))
#define MODIFY_REG(REG, CLEARMASK, SETMASK)  WRITE_REG((REG), (((READ_REG(REG)) & (~(CLEARMASK))) | (SETMASK)))

#include "../1km/driver/io_expand/io_1061s.h"

/*IO子板列表*/
static io_1061s_t io_1061s_table[] = {
    [0] = {
        .node_id = 0x01,
        .default_out = 0x00FFFFFF,
    },
    [1] = {
        .node_id = 0x02,
        .default_out = 0x00FFFFFF,
    },
    [2] = {
        .node_id = 0x03,
        .default_out = 0x00FFFFFF,
    },
    [3] = {
        .node_id = 0x04,
        .default_out = 0x00FFFFFF,
    },
    [4] = {
        .node_id = 0x05,
        .default_out = 0x00FFFFFF,
    },
    [5] = {
        .node_id = 0x06,
        .default_out = 0x00FFFFFF,
    },
};
void xp_io_event_callback(uint8_t board_id, uint8_t event_id, void *event_data){
    switch(event_id){
        case EVENT_LINK:{
            uint32_t is_online = *(uint32_t *)event_data;
            io_1061s_table[board_id - 1].is_online = is_online;
            println("io board <%d> is %s\r\n", board_id, is_online ? ("OnLine") : ("OffLine"));
        }
        break;

        case EVENT_INPUT_CHANGE:{
            uint32_t *io_state = (uint32_t *)event_data;
            // println("io board <%d> input change: 0x%04X cur: 0x%04X\r\n", board_id, io_state[2], io_state[0]);
        }
        break;
    }
	
}
int xp_io_manage_init(u8 num){
    int ret = -1;
    if (io_1061s_init(io_1061s_table, num, xp_io_event_callback)){
        for (uint8_t i = 0; i < num; i++){
            uint8_t tryTimes = 3;
            ret = -1;
            while(tryTimes-- && ret){
                aos_msleep(40);		// 由于初始化完成后子板并不是立刻在线，必须等待子板在线才可以配置参数
                ret = 0;
                // 这里仅是假设num块子板的配置参数一致，否则需要单独配置
                ret += io_1061s_set_over_time(i, 500);
                ret += io_1061s_set_default_output(i, 0x00FFFFFF);
            }
            // if(0 == ret){           //参数配置成功说明子板在线，继续配置设备控制引脚（不管默认值设置成不成功都继续配置，不然子板中途恢复上线后会因为没注册而读不到信息）
                for (uint8_t j = 0; j < crlDev[i].configDevCrlNum; j++)
                {
                    println("set board %d, dev %d, pin_zero %d, pin_pulse %d, pin_CW %d, pin_CCW %d", \
                    i, j, crlDev[i].pin_zero[j], crlDev[i].pin_pulse[j], crlDev[i].pin_CW[j], crlDev[i].pin_CCW[j]);
                    crlDev[i].configData[j] = ((uint32_t)crlDev[i].pin_zero[j]) << 24;
                    crlDev[i].configData[j] = crlDev[i].configData[j] | (((uint32_t)crlDev[i].pin_pulse[j] << 16) & 0x00FFFFFF);
                    crlDev[i].configData[j] = crlDev[i].configData[j] | (((uint32_t)crlDev[i].pin_CW[j] << 8) & 0x0000FFFF);
                    crlDev[i].configData[j] = crlDev[i].configData[j] | (((uint32_t)crlDev[i].pin_CCW[j]) & 0x000000FF);
                    _io_1061s_config_dev_crl_pin(i, j, crlDev[i].configData[j]);
                }
                println("board %d, register %d dev", i, crlDev[i].configDevCrlNum);
            // }
        }
    }

    if(0 != ret){
        perr("io board init fail");
        return -1;
    }
    return ret;
}
void xp_io_write_pin(const uint8_t board_id,const uint8_t pin,const bool state){
    uint32_t tmp;
    io_1061s_get_output_pins(board_id - 1, &tmp);
    MODIFY_REG(tmp, 1 << pin, state ? (1 << pin) : (0));
	io_1061s_set_output_pins(board_id - 1, tmp);
}
void xp_io_write_pins(const u8 board_id,u32 states,u8 start_pin,u8 pins){
    uint32_t tmp;
    io_1061s_get_output_pins(board_id - 1, &tmp);
    MODIFY_REG(tmp, ((1 << pins) - 1) << start_pin, (states & ((1 << pins) - 1)) << start_pin);
    io_1061s_set_output_pins(board_id - 1, tmp);
}
u8 xp_io_read_input_pin(const u8 board_id,const u8 pin){
    uint32_t tmp = 0;
    io_1061s_get_input_pins(board_id - 1, &tmp);
    return (tmp & (1 << pin)) ? (1) : (0);
}
int xp_io_read_input_pins(const u8 board_id,u32 *states,u8 start_pin,u8 pins){
    uint32_t tmp = 0;
    io_1061s_get_input_pins(board_id - 1, &tmp);
    *states = tmp;
    return 0;
}
u8 xp_io_read_output_pin(const u8 board_id,const u8 pin){
    uint32_t tmp = 0;
    io_1061s_get_output_pins(board_id - 1, &tmp);
    return (tmp & (1 << pin)) ? (1) : (0);
}
u8 xp_io_read_output_pins(const u8 board_id,u32 *states,u8 start_pin,u8 pins){
    uint32_t tmp = 0;
    io_1061s_get_output_pins(board_id - 1, &tmp);
    return (tmp >> start_pin) & ((1 << pins) - 1);
}
u8 xp_io_read_output_pin_real(const u8 board_id,const u8 pin){
    uint32_t tmp = 0;
    io_1061s_get_output_pins_real(board_id - 1, &tmp);
    return (tmp & (1 << pin)) ? (1) : (0);
}
u32 xp_io_read_output_pins_real(const u8 board_id,u8 start_pin,u8 pins){
    uint32_t tmp = 0;
    io_1061s_get_output_pins_real(board_id - 1, &tmp);
    return (tmp >> start_pin) & ((1 << (pins + 1)) - 1);
}
int xp_output_set_safe(const u8 board_id,u32 states,u8 start_pin,u8 pins){
	(void) start_pin;
	(void) pins;
	return io_1061s_set_default_output(board_id - 1, states);
}
// int xp_io_set_pulse_mode(uint8_t board_id, uint32_t pin_bit_id){
//     return io_1061s_set_pulse(board_id - 1, pin_bit_id, 0);
// }
int xp_io_clear_pulse(uint8_t board_id, uint32_t pin_id){
    return io_1061s_clear_pulse(board_id - 1, pin_id);
}
int xp_io_get_pulse(uint8_t board_id, uint8_t pin_id, uint32_t * const pResult){
    return io_1061s_get_pulse(board_id - 1, pin_id, pResult);
}


/**
 * @brief       配置由子板控制的设备相关引脚
 * @param[in]	board_id            子板ID
 * @param[in]	pin_zero            设备零点信号
 * @param[in]	pin_pulse           设备脉冲计数信号
 * @param[in]	pin_CW              设备正转信号
 * @param[in]	pin_CCW             设备反转信号
 * @return      int                 
 */
int xp_io_config_crl_pin(uint8_t board_id, uint8_t pin_zero, uint8_t pin_pulse, uint8_t pin_CW, uint8_t pin_CCW){
    uint8_t configDevNum;

    configDevNum = crlDev[board_id - 1].configDevCrlNum;
    if(configDevNum >= MAX_REG_DEV_NUMS){
        perr("illegal configDevNum");
        return -1;
    }

    crlDev[board_id - 1].pin_zero[configDevNum]  = pin_zero;
    crlDev[board_id - 1].pin_pulse[configDevNum] = pin_pulse;
    crlDev[board_id - 1].pin_CW[configDevNum]    = pin_CW;
    crlDev[board_id - 1].pin_CCW[configDevNum]   = pin_CCW;
    crlDev[board_id - 1].configDevCrlNum++;            //计算这块子板配置了几个设备控制
}

int xp_io_set_dev_move_time(uint8_t board_id, uint8_t dev_id, int8_t dir, uint32_t time_ms){
    time_ms = (time_ms > BOARD_IO_CRL_MAX_DATA) ? BOARD_IO_CRL_MAX_DATA : time_ms;
    while(get_set_move_time_flag()){        //一直等待上一次设置结束，防止函数重入
        aos_msleep(3);
    };
    return io_1061s_set_dev_move_time(board_id - 1, dev_id, dir, time_ms);
}

int xp_io_set_dev_move_pos(uint8_t board_id, uint8_t dev_id, int32_t move_pos){
    move_pos = (move_pos > BOARD_IO_CRL_MAX_DATA) ? BOARD_IO_CRL_MAX_DATA : move_pos;
    while(get_set_move_pos_flag()){         //一直等待上一次设置结束，防止函数重入
        aos_msleep(3);
    };
    return io_1061s_set_dev_move_pos(board_id - 1, dev_id, move_pos);
}

int32_t xp_io_get_dev_encoder(uint8_t board_id, uint8_t dev_id, int32_t * const pResult){
    while(get_encoder_flag()){              //一直等待上一次设置结束，防止函数重入
        aos_msleep(3);
    };
    return io_1061s_get_dev_encoder(board_id - 1, dev_id, pResult);
}

int xp_io_clear_encoder(uint8_t board_id, uint8_t dev_id){
    return io_1061s_clear_encoder(board_id - 1, dev_id);
}

uint8_t xp_io_get_register_dev_num(uint8_t board_id){
    return crlDev[board_id - 1].configDevCrlNum;
}

bool xp_io_get_online_sta(uint8_t board_id)
{
    return io_1061s_table[board_id - 1].is_online;
}


#else
/*使用旧版io子板程序,canopen*/
/**************************************************************************************************/
/*
io board online state check
param:
    board_id:io board id
    state:online state
*/
void xp_io_board_state_check(const uint8_t board_id,const io_board_state_t state){
    char buf[64];
	
    memset(buf,0,sizeof(buf));
    switch (state)
    {
    case io_board_working:
        inquire_io_board_version(board_id,buf,sizeof(buf));
        println("%d io board working,input cnt %d,output cnt %d,version:%s",
                board_id,
                get_io_board_input_cnt(board_id),	//get input port num
                get_io_board_output_cnt(board_id),	//get output port num
                buf);
        is_io_board_working = true;
        break;
    case io_board_lost_connection:
        is_io_board_working = false;
        println("%d io board lost connection!",board_id);
    default:
        is_io_board_working = false;
        break;
    }
}

/*
io board input state change callback
param:
    board_id:io board id
    len: board io group number，one group = 8 io
    state:io state array
*/
void xp_input_change_callback(const uint8_t board_id,const uint8_t len,const uint8_t *state){
	u32 sta=0;
    for(uint8_t i=0;i<len;i++){
		sta|=(state[i]<<(i*8));
    }
	// println("%d io input changed:0x%x",board_id,sta);
	
	//add your io input event deal code
	xp_input_event_deal(board_id,sta);
	
	//end your code
}



/*
io manage init
return:
    0=success
*/
int xp_io_manage_init(u8 num){
    int count=0;
    static int first=1;

    if(io_board_manager_init(xp_io_board_state_check,xp_input_change_callback)!=0){
        return -1;
    }
    //set io default state
    while(1){
        if(is_io_board_working==true){
            aos_msleep(100);
            if(is_io_board_working==true)break;
        }
        count++;
        if((count%10)==0){
            println("Wait io exp board connect !");
        }
        else if(count>50){
            println("IO exp board can't connet over 10s. return fail. Need check wire !");
            if(first==1){
                aos_task_new("io_connect_try",xp_io_board_tryConnect,&num,2000);
                first=0;
            }
            return -2;
        }
        aos_msleep(100);
    }
    println("IO exp board connected !");
    for(u8 i=1;i<=num;i++){
        count=0;
        while(xp_io_DO_default_set(i,1)!=0){
            count++;
            if(count>6){
                perr("io board %d init fail,will restart~~", i);
                // aos_reboot();
                if(first==1){
                    aos_task_new("io_connect_try",xp_io_board_tryConnect,&num,2000);
                    first=0;
                }
                return -2;
            }
            aos_msleep(500);
        }
        aos_msleep(100);
    }
    xp_output_default_set();

    //end
    return 0;
}


/*
io output set
param:
	board_id:io board id
    pin:io pin number
    state:set io state
*/
void xp_io_write_pin(const uint8_t board_id,const uint8_t pin,const bool state){
    u8 retry_cnt = 5;

    while(retry_cnt--){
        if(0 == gpio_output(board_id,pin,state)) break;
        else  println("io set error,board %d,pin %d,state %d,try_cnt %d",board_id,pin,state,retry_cnt);
        // aos_msleep(10);
    }
}



/*
io write pins
param:
    board_id: io board id
    states:io states
    start_pin: start pin: 0-23
    pins:pin number
*/
void xp_io_write_pins(const u8 board_id,u32 states,u8 start_pin,u8 pins){
    set_gpio_output_batch(board_id,(u8 *)&states,start_pin,pins);
}

/*
return -1 == fail
*/
int xp_io_write_pins_rt(const u8 board_id,u32 states,u8 start_pin,u8 pins){
    return set_gpio_output_batch(board_id,(u8 *)&states,start_pin,pins);
}

/*
get io input state
param:
    board_id:io board id
    pin:pin number
return:pin state
*/
u8 xp_io_read_input_pin(const u8 board_id,const u8 pin){
    if(board_id!=0){
        return inquire_gpio_input(board_id, pin);
    }

}

/*
get board all io inputs state, add by maoxinfeng
param:
    board_id:io board id
return:pin state
*/
int xp_io_read_input_pins(const u8 board_id,u32 *states,u8 start_pin,u8 pins){
    if(board_id!=0){
        return inquire_gpio_input_batch(board_id,states,start_pin,pins);
    }

}

/*
get io output state
param:
    board_id:io board id
    pin:pin number
return:pin state
*/
u8 xp_io_read_output_pin(const u8 board_id,const u8 pin){
    return inquire_gpio_output(board_id,pin);
}

/*
get io outputs state
param:
    board_id:io board id
    pin:pin number
return:pin state
*/
u8 xp_io_read_output_pins(const u8 board_id,u32 *states,u8 start_pin,u8 pins){
    int rc = 0;
    uint8_t bit_pos = 0;

    for(bit_pos = start_pin; bit_pos < start_pin+pins; bit_pos++){
        rc = inquire_gpio_output(board_id,bit_pos);
        *states |= rc << (bit_pos - start_pin);
    }
    return 0;
}

/*
io write pins safe
param:
    board_id: io board id
    states:io states
    start_pin: start pin
    pins:pin number
*/
int xp_output_set_safe(const u8 board_id,u32 states,u8 start_pin,u8 pins){
	uint32_t pin_act_states = 0;
	uint64_t start_time = 0;

	println("xp_output_set_safe: board_id %d states 0x%x start_pin %d pins %d", board_id, states, start_pin, pins);

	start_time = aos_now_ms();
	while(1){
		xp_io_write_pins(board_id, states, start_pin, pins);
		aos_msleep(200);
		xp_io_read_output_pins(board_id, &pin_act_states, start_pin, pins);
		println("pin_act_states 0x%x", pin_act_states);
		//if(pin_act_states == states){
			return 0;
		//}
		if(get_diff_ms(start_time) > 2000){
			println("xp_output_set_safe timeout! check io board communication!");
			return -1;
		}
	}
}
#endif


bool io_board_working_state_get(void) {
    return is_io_board_working;
}

int xp_output_test()
{
	uint32_t pin_act_states = 0;
	uint64_t start_time = aos_now_ms();

	while(1){
		xp_io_write_pins(2, 0x5, 0, 4);
		aos_msleep(200);
		xp_io_read_output_pins(2, &pin_act_states, 0, 4);
		println("pin_act_states 0x%x", pin_act_states);
		if(pin_act_states == 0x5){
			return 0;
		}
		if(get_diff_ms(start_time) > 2000){
			println("xp_output_set_safe timeout! check io board communication!");
			xp_io_read_output_pins(2, 0xf, 0, 4);
			return -1;
		}
	}
}

/*
test DO set
*/
void xp_DO_test(int blink){
    u32 flag=0x01;
    u32 status;

    if(blink==0){
        blink=10;
    }
    for(int i=0;i<24;i++){
        status=~(flag<<i);
        xp_io_write_pins(2,status,0,24);
        aos_msleep(blink);
    }
    xp_io_write_pins(2,0xffffff,0,24);
}

/*
io init 
param:
    gpio:gpio object
    io:io port mode string
    num:board io num
return:
    0=success
*/
int xp_io_init(gpio_dev_t *gpio,char *io,u8 num){
    int sta=0;
    if(gpio==NULL)return -1;
    sta=hal_gpio_init(gpio);
    if(sta!=0){
        println("%s %d init fail~ code=%d",io,num,sta);
    }
    return sta;
}


/*
io default output set
param:
    sta:state
*/
int xp_io_DO_default_set(u8 id,u8 sta){
    uint8_t def_output_setting[4] = {0};

    if(sta==0){
        memset(def_output_setting,0,4);
    }
    else{
        memset(def_output_setting,0xff,4);
    }
    return set_gpio_output_default(id, sizeof(def_output_setting), def_output_setting);
}


/*
set io board power manage api
*/
void xp_io_boardPowerAPI_set(void (*api)(void)){
    if(api==NULL){
        return;
    }
    api();
    io_boardPower_manage=api;
}

/*
connect io board thread
*/
void xp_io_board_tryConnect(void *arg){
    int num=*(int*)arg;

    while(1){
        if(xp_io_manage_init(num)!=0){
            if(io_boardPower_manage!=NULL){
                io_boardPower_manage();
                continue;
            }
        }
        else{
            break;
        }
        aos_msleep(1000);
    }
    aos_task_exit(0);
}


/*****************callback weak define*********************/
void  __attribute__((weak)) xp_output_default_set(void){

    println("Output default set work in weak function!");
}

void __attribute__((weak)) xp_input_event_deal(const uint8_t board_id,const u32 io_state){
    // println("Input event deal work in weak function!");
}


/********************io debug***********************/
/*
io debug
*/
int xp_io_debug(char *type,char *fun,char *param){
    int ret = -1;
	uint8_t board_id;
    u32 state;

	//本调试接口仅支持 "io_" 起始的命令
	if (NULL == strstr(type, "io_")){
		return 0;
	}else{
		type += strlen("io_");
        board_id = atoi(type);
        if (board_id < 1){
            return 0;
        }
	}
    //xp shihlin_show  打印所测量信息
	if (strcasecmp(fun, "show") == 0){
		
	}
    else if (strcasecmp(fun, "set") == 0){
		xp_io_write_pin(board_id, atoi(param), 1);
	}
    else if (strcasecmp(fun, "reset") == 0){
		xp_io_write_pin(board_id, atoi(param), 0);
	}
    else if (strcasecmp(fun, "write_pins") == 0){
		xp_io_write_pins(board_id, 1, 0, atoi(param));
	}
    else if (strcasecmp(fun, "read_in_pin") == 0){
		printf("board_id: %d in_pin %d is %x\r\n", board_id, atoi(param), xp_io_read_input_pin(board_id, atoi(param)));
	}
    else if (strcasecmp(fun, "read_in_pins") == 0){
        printf("board_id: %d in_start_pin %d pin_num %d is %x\r\n", board_id, 0, atoi(param), xp_io_read_input_pins(board_id, &state, 0, atoi(param)));
	}
    else if (strcasecmp(fun, "read_out_pin") == 0){
		printf("board_id: %d out_pin %d is %x\r\n", board_id, atoi(param), xp_io_read_output_pin(board_id, atoi(param)));
	}
    else if (strcasecmp(fun, "read_out_pins") == 0){
        printf("board_id: %d in_start_pin %d pin_num %d is %x\r\n", board_id, 0, atoi(param), xp_io_read_output_pins(board_id, &state, 0, atoi(param)));
	}
    else {
        return 0;
    }
    return 1;
}


//end of the file


